/* 	This class defines the formation structure */

#ifndef _FORMATION_STRUCTION_H
#define _FORMATION_STRUCTION_H

#include <iostream>
#include <ros/ros.h>
#include <Eigen/Eigen>

using namespace std;
namespace  formation_avoidance{


  enum formation {
    Diamond = 0,
	Rectangle,
	Hexagon,
	Vertical_Line
  };

class Formation_Structure
{
public:
	std::vector<Eigen::Matrix3d> trans_matrix;
	std::vector<Eigen::Vector3d> agent_pos;
	int formation_agent_num;
	double base_ratio=1;
	double safefy_formation_threshold=0;
	double base_p0p1;
	formation formation_type;
	Eigen::Vector3d agent_p0,agent_p1;
public:
	Formation_Structure()
	{



	}

	void formation_structure_init(formation set_formation_structure, double set_base_ratio, double set_safefy_formation_threshold, double set_base_p0p1)
	{
		base_ratio=set_base_ratio;
		safefy_formation_threshold=set_safefy_formation_threshold;
		base_p0p1=set_base_p0p1;
		formation_type=set_formation_structure;

		double theta;

		Eigen::AngleAxisd t_V(0, Eigen::Vector3d(0, 0, 1));

		if(set_formation_structure==formation::Diamond)
		{
			formation_agent_num=4;
			trans_matrix.resize(formation_agent_num);
			agent_pos.resize(formation_agent_num);

			trans_matrix[0]=Eigen::Matrix3d::Identity();
			trans_matrix[1]=Eigen::Matrix3d::Identity();
			t_V.angle()=std::acos(0.5/base_ratio);
			trans_matrix[2]=t_V.toRotationMatrix()*base_ratio;

			t_V.angle()=-std::acos(.5/base_ratio);
			trans_matrix[3]=t_V.toRotationMatrix()*base_ratio;
		}

		if(set_formation_structure==formation::Rectangle)
		{
			formation_agent_num=4;
			trans_matrix.resize(formation_agent_num);
			agent_pos.resize(formation_agent_num);

			trans_matrix[0]=Eigen::Matrix3d::Identity();
			trans_matrix[1]=Eigen::Matrix3d::Identity();


			t_V.angle()=std::atan(base_ratio);
			trans_matrix[2]=t_V.toRotationMatrix()*sqrt(base_ratio*base_ratio+1);

			t_V.angle()=M_PI_2;
			trans_matrix[3]=t_V.toRotationMatrix()*base_ratio;
		}


		if(set_formation_structure==formation::Hexagon)
		{
			formation_agent_num=6;
			trans_matrix.resize(formation_agent_num);
			agent_pos.resize(formation_agent_num);			
			
			trans_matrix[0]=Eigen::Matrix3d::Identity();
			trans_matrix[1]=Eigen::Matrix3d::Identity();
			t_V.angle()=atan(sqrt(3)*base_ratio/(base_ratio+2));
			trans_matrix[2]=t_V.toRotationMatrix()*sqrt(0.75*base_ratio*base_ratio+pow(0.5*base_ratio+1,2));


			t_V.angle()=atan(sqrt(3)*base_ratio);
			trans_matrix[3]=t_V.toRotationMatrix()*sqrt(3*base_ratio*base_ratio+1);

			t_V.angle()=M_PI_2;
			trans_matrix[4]=t_V.toRotationMatrix()*sqrt(3*base_ratio*base_ratio);			

			t_V.angle()=M_PI*0.666667;
			trans_matrix[5]=t_V.toRotationMatrix()*base_ratio;	

		}



		if(set_formation_structure==formation::Vertical_Line)
		{
			formation_agent_num=4;
			trans_matrix.resize(formation_agent_num);
			agent_pos.resize(formation_agent_num);			
			
			trans_matrix[0]=Eigen::Matrix3d::Identity();
			trans_matrix[1]=Eigen::Matrix3d::Identity();
			t_V.angle()=0;
			trans_matrix[2]=t_V.toRotationMatrix()*2;

			t_V.angle()=0;
			trans_matrix[3]=t_V.toRotationMatrix()*3;
		}


	}

	void set_p0_p1(Eigen::Vector3d pos0, Eigen::Vector3d pos1)
	{
		agent_p0=pos0;
		agent_p1=pos1;
	}

	std::vector<Eigen::Vector3d> calculateAllPosition(Eigen::Vector3d pos0, Eigen::Vector3d pos1)
	{
		set_p0_p1(pos0,pos1);
		std::vector<Eigen::Vector3d >pos;
		pos.push_back(agent_p0);
		pos.push_back(agent_p1);

		for(int id=2;id<formation_agent_num;id++)
		{
			pos.push_back(trans_matrix[id]*(agent_p1-agent_p0)+agent_p0);
		}
		return pos;
	}


	std::vector<Eigen::Vector3i> calculateCentreOffset()
	{
		std::vector<Eigen::Vector3i >offset;
		offset.resize(formation_agent_num);
		
		Eigen::Vector3i agent_offset;
		if(formation_type==formation::Diamond)
		{

			Eigen::Vector3d pos0(base_p0p1/2,0,0);
			Eigen::Vector3d pos1(-base_p0p1/2,0,0);

			std::vector<Eigen::Vector3d> all_position=calculateAllPosition(pos0,pos1);

			for(int i=0;i<formation_agent_num;i++)
			{
				offset[i]=(all_position[i]*10*0.35).cast<int>();
			}

			offset[0]<<2,0,0;
			offset[1]<<-2,0,0;
			offset[2]<<0,-2,0;
			offset[3]<<0,2,0;

		}
		
		if(formation_type==formation::Rectangle)
		{
			Eigen::Vector3d pos0(0,0,0);
			Eigen::Vector3d pos1(0,-base_p0p1,0);

			std::vector<Eigen::Vector3d> all_position=calculateAllPosition(pos0,pos1);

			Eigen::Vector3d centre=(all_position[0]+all_position[2])/2;

			for(int i=0;i<formation_agent_num;i++)
			{
				offset[i]=((all_position[i]-centre)*10*0.2).cast<int>();
			}
		}


		if(formation_type==formation::Vertical_Line)
		{
			Eigen::Vector3d pos0(0,0,0);
			Eigen::Vector3d pos1(-base_p0p1,0,0);

			std::vector<Eigen::Vector3d> all_position=calculateAllPosition(pos0,pos1);

			Eigen::Vector3d centre=(all_position[0]+all_position[3])/2;

			for(int i=0;i<formation_agent_num;i++)
			{
				offset[i]=((all_position[i]-centre)*10*0.3).cast<int>();
			}
		}


		return offset;
	}

	~Formation_Structure(){}






};

}






#endif